clc;
clear;
close all;
% syms theta_1 theta_2 theta_3 l1 l2 l3 x3 y3
% 
% eq1 = l3*cos(theta_3)+l2*cos(theta_1+theta_2)==l1*cos(theta_1)-x3;
% eq2 = l3*sin(theta_3)+l2*sin(theta_1+theta_2)==l1*sin(theta_1)-y3;
% 
% sol = solve([eq1, eq2], [theta_2, theta_3]);
% disp(sol)

addpath('autogen\')

t_1=0.3151;
t_2=pi-1.1642;
t_3=0.4545; 
l1=0.0185;
l2=0.04856;
l3=0.01237;
x3=0.03856;
y3=-0.03615;

% e1 = l3*cos(t_3)+l2*cos(t_1+t_2);
% e2 = l1*cos(t_1)-x3;

% e3 = l3*sin(t_3)+l2*sin(t_1+t_2);
% e4 = l1*sin(t_1)-y3;

t2_m = theta_2(t_1, t_2, t_3, l1, l2, l3, x3, y3);
t3_m = theta_3(t_1, t_2, t_3, l1, l2, l3, x3, y3);

m = l1*cos(t_1)-x3;
n = l1*sin(t_1)-y3;
A = l3;B = l2;

t2_d(1) = atan2(n, m)+acos((m^2+n^2+A^2-B^2)/(2*A*sqrt(m^2+n^2)));
t2_d(2) = atan2(n, m)-acos((m^2+n^2+A^2-B^2)/(2*A*sqrt(m^2+n^2)));

t3_d(1) = atan2(n, m)-acos((m^2+n^2-A^2+B^2)/(2*B*sqrt(m^2+n^2)))-t_1;
t3_d(2) = atan2(n, m)+acos((m^2+n^2-A^2+B^2)/(2*B*sqrt(m^2+n^2)))-t_1;